//#include <stdio.h>
#include <malloc.h>
#include <math.h>
#include "../../include/sensors/LSM303DLH_m.h"
#include "../../include/sensors/cal_sensors.h"

void Multiply_Matrices(double *C, double *A, int nrows, int ncols, double *B, int mcols);

/* TODO vérifier ce qu'il y a à "reseter" */
void reset_values_mag(mag_sensor *mag)
{
	mag->calibration.bias[0] = 0;
	mag->calibration.bias[1] = 0;
	mag->calibration.bias[2] = 0;
	mag->calibration.A_1[0] = 0;
	mag->calibration.A_1[1] = 0;
	mag->calibration.A_1[2] = 0;
	mag->calibration.A_1[3] = 0;
	mag->calibration.A_1[4] = 0;
	mag->calibration.A_1[5] = 0;
}

int Mag_Init(mag_sensor *mag){
    FILE *fmag;
    int errno;
    fmag = fopen(MAG_Name_file, "r"); /* TODO attention il s'agit du nom pas d'ID de dispo... peut etre le meme que accéléromètre */
    if(fmag < 0)
    {
        printf("failed to open file, error %d\n", errno);
        return -1;
    }
    /* TODO test sur chaque lecture de fichier... */
    fscanf(fmag, "%s",mag->name);
    printf("sensor name: %s\n",mag->name);
    fclose(fmag);

    fmag = fopen(MAG_Mode_file, "w+");
    if(fmag < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    fprintf(fmag,"%d",LSM303DLH_M_MODE_CONTINUOUS);
    fseek(fmag, 0, SEEK_SET);
    fscanf(fmag,"%d",&mag->config.mode);
    fclose(fmag);
    mag->config.mode=LSM303DLH_M_MODE_CONTINUOUS;
    fmag = fopen(MAG_Rate_file, "r");
    if(fmag < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    if((fscanf(fmag, "%x",&mag->config.rate))<1)
        printf("failed to read sensor rate\n");
    fclose(fmag);
    fmag = fopen(MAG_Gain_file, "r");
    if(fmag < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    if((fscanf(fmag, "%x:%x:%x",&mag->config.gain[0],&mag->config.gain[1],&mag->config.gain[2]))!=3)
        printf("failed to read sensor gain\n");
    fclose(fmag);
    fmag = fopen(MAG_Range_file, "r");
    if(fmag < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    if((fscanf(fmag, "%d",&mag->config.range)<1))
        printf("failed to read sensor range\n");
    fclose(fmag);
    return 0;

}

/* structure de donées pour la configuration du MEMs (rate, gain, mode, ...) */
/* TODO revoir les IF sans doute il faut mettre des else */
int Mag_write_config(mag_sensor *mag)
{
    FILE *fmag;
    int errno;
    fmag = fopen(MAG_Mode_file, "w+");
    if(fmag < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    fprintf(fmag,"%d",mag->config.mode);
    fclose(fmag);

    fmag = fopen(MAG_Rate_file, "w+");
    if(fmag < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    fprintf(fmag,"%d",mag->config.rate);
    fclose(fmag);

    fmag = fopen(MAG_Range_file, "w+");
    if(fmag < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    fprintf(fmag,"%d",mag->config.range);
    fclose(fmag);
    /* pas besoin d'écrire le gain il se met automatiquement à jour (normalement) */
    return 0;
}

int Mag_read_config(mag_sensor *mag)
{
    FILE *fmag;
    int errno;
    fmag = fopen(MAG_Name_file, "r");
    if(fmag < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    fscanf(fmag, "%s",mag->name);
    printf("sensor name: %s\n",mag->name);
    fclose(fmag);

    fmag = fopen(MAG_Mode_file, "r");
    if(fmag < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    if((fscanf(fmag, "%d",&mag->config.mode))<1)
        printf("failed to read sensor rate\n");
    fclose(fmag);

    fmag = fopen(MAG_Range_file, "r");
    if(fmag < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    if((fscanf(fmag, "%d",&mag->config.range))<1)
        printf("failed to read sensor range\n");
    fclose(fmag);

    fmag = fopen(MAG_Rate_file, "r");
    if(fmag < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    if((fscanf(fmag, "%d",&mag->config.rate))<1)
        printf("failed to read sensor rate");
    fclose(fmag);
    fmag = fopen(MAG_Gain_file, "r");
    if(fmag < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    if((fscanf(fmag, "%x:%x:%x",&mag->config.gain[0],&mag->config.gain[1],&mag->config.gain[2]))<3)
    {
    	printf("failed to read sensor gain");
    	return -1;
    }

    fclose(fmag);
    return 0;
}

/*TODO fonction de lecture des données du magnétomètre */
/* verifier que la data est bien dispo sinon en renvoie la même donnée
    peut se faire sans doute avec IRQ, à étudier... */
int Mag_read_data(mag_sensor *mag)
{
    int errno,ret;
    int X,Y,Z;
    FILE *fmag;
    /* verifier que le magnétomètre est en marche ??? ou faire dans fonction au dessus... */
    fmag = fopen(MAG_Data_file, "r");
    if(fmag<0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    fseek(fmag, 0, SEEK_SET);
    /* TODO attention recompiler le noyau avec driver composant LSM303DLMHC */
    ret = fscanf(fmag, "%x:%x:%x", &Y,&X,&Z);//&mag->raw.axis[0], &mag->raw.axis[1], &mag->raw.axis[2]);
    if (ret<3)
    {
    	printf("failed to read data mag: %d\n",ret);
        return -1;
    }

    /* todo #define ou recompiler noyau avec driver LSM303DLHC */
    mag->raw.axis[0] = X * 1000 / mag->config.gain[0]; /* TODO: verifier la formule et format de la donnée... */
    mag->raw.axis[1] = Y * 1000 / mag->config.gain[1];
    mag->raw.axis[2] = Z * 1000 / mag->config.gain[2];

    fclose(fmag);
    return 0;
}

void scaled_data(mag_sensor *mag)
/* heading data tilt compensated after calibration and after pitch and roll calculation */
{
    double *M = (double*)malloc(sizeof(mag->raw.axis) * sizeof(double));

    /* normalized data (calibrated data) */
    double norm = sqrt(mag->raw.axis[0]*mag->raw.axis[0]+mag->raw.axis[1]*mag->raw.axis[1]+mag->raw.axis[2]*mag->raw.axis[2]);
    mag->scaled.axis[0]=(mag->raw.axis[0]/norm);
    mag->scaled.axis[1]=(mag->raw.axis[1]/norm);
    mag->scaled.axis[2]=(mag->raw.axis[2]/norm);


    Subtract_Matrices(M,mag->raw.axis,mag->calibration.bias,1,3);
    //printf("rawX:%lf\trawY:%lf\trawZ:%lf\n",mag->raw.axis[0],mag->raw.axis[1],mag->raw.axis[2]);
    //printf("biasX:%lf\tbiasY:%lf\tbiasZ:%lf\n",mag->calibration.bias[0],mag->calibration.bias[1],mag->calibration.bias[2]);
    //printf("X:%lf\tY:%lf\tZ:%lf\n",M[0],M[1],M[2]);
    //for(i = 0; i < 3; i++)
    //    printf("%lf %lf %lf\r\n", mag->calibration.A_1[i*3], mag->calibration.A_1[i*3+1], mag->calibration.A_1[i*3+2]);
    Multiply_Matrices(mag->scaled.axis,M,1,3,mag->calibration.A_1,3);
    //printf("scaledX:%lf\tScaledY:%lf\tScaledZ:%lf\n",mag->scaled.axis[0],mag->scaled.axis[1],mag->scaled.axis[2]);

}


